/**
 ******************************************************************************
 * @addtogroup OpenPilotModules OpenPilot Modules
 * @{
 * @addtogroup FlightPlan Flight Plan Module
 * @brief Executes flight plan scripts in Python
 * @{
 *
 * @file       flightplan.c
 * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
 * @brief      Executes flight plan scripts in Python
 *
 * @see        The GNU Public License (GPL) Version 3
 *
 *****************************************************************************/
/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
 * for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 */

#include <openpilot.h>

#include "flightplanstatus.h"
#include "flightplancontrol.h"
#include "flightplansettings.h"
#include "taskinfo.h"

#include "pm.h"

// Private constants
#define STACK_SIZE_BYTES 1500
#define TASK_PRIORITY    (tskIDLE_PRIORITY + 1)
#define MAX_QUEUE_SIZE   2

// Private types

// Private variables
static xTaskHandle taskHandle;
static xQueueHandle queue;

// Private functions
static void flightPlanTask(void *parameters);
static void objectUpdatedCb(UAVObjEvent *ev);

// External variables (temporary, TODO: this will be loaded from the SD card)
extern unsigned char usrlib_img[];

/**
 * Module initialization
 */
int32_t FlightPlanStart()
{
    taskHandle = NULL;

    // Start VM thread
    xTaskCreate(flightPlanTask, (signed char *)"FlightPlan", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
    PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_FLIGHTPLAN, taskHandle);

    return 0;
}

/**
 * Module initialization
 */
int32_t FlightPlanInitialize()
{
    taskHandle = NULL;

    FlightPlanStatusInitialize();
    FlightPlanControlInitialize();
    FlightPlanSettingsInitialize();

    // Listen for object updates
    FlightPlanControlConnectCallback(&objectUpdatedCb);

    // Create object queue
    queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));

    // Listen for FlightPlanControl updates
    FlightPlanControlConnectQueue(queue);

    return 0;
}
MODULE_INITCALL(FlightPlanInitialize, FlightPlanStart);
/**
 * Module task
 */
static void flightPlanTask(__attribute__((unused)) void *parameters)
{
    UAVObjEvent ev;
    PmReturn_t retval;
    FlightPlanStatusData status;
    FlightPlanControlData control;

    // Setup status object
    status.Status       = FLIGHTPLANSTATUS_STATUS_STOPPED;
    status.ErrorFileID  = 0;
    status.ErrorLineNum = 0;
    status.ErrorType    = FLIGHTPLANSTATUS_ERRORTYPE_NONE;
    status.Debug[0]     = 0.0;
    status.Debug[1]     = 0.0;
    FlightPlanStatusSet(&status);

    // Main thread loop
    while (1) {
        // Wait for FlightPlanControl updates
        while (xQueueReceive(queue, &ev, portMAX_DELAY) != pdTRUE) {
            ;
        }

        // Get object and check if a start command was sent
        FlightPlanControlGet(&control);
        if (control.Command == FLIGHTPLANCONTROL_COMMAND_START) {
            // Init PyMite
            retval = pm_init(MEMSPACE_PROG, usrlib_img);
            if (retval == PM_RET_OK) {
                // Update status
                FlightPlanStatusGet(&status);
                status.Status = FLIGHTPLANSTATUS_STATUS_RUNNING;
                FlightPlanStatusSet(&status);
                // Run the test script (TODO: load from SD card)
                retval = pm_run((uint8_t *)"test");
                // Check if an error or exception was thrown
                if (retval == PM_RET_OK || retval == PM_RET_EX_EXIT) {
                    status.Status    = FLIGHTPLANSTATUS_STATUS_STOPPED;
                    status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_NONE;
                } else if (retval == PM_RET_EX) {
                    status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
                    status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_EXCEPTION;
                } else if (retval == PM_RET_EX_IO) {
                    status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
                    status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_IOERROR;
                } else if (retval == PM_RET_EX_ZDIV) {
                    status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
                    status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_DIVBYZERO;
                } else if (retval == PM_RET_EX_ASSRT) {
                    status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
                    status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_ASSERTERROR;
                } else if (retval == PM_RET_EX_ATTR) {
                    status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
                    status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_ATTRIBUTEERROR;
                } else if (retval == PM_RET_EX_IMPRT) {
                    status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
                    status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_IMPORTERROR;
                } else if (retval == PM_RET_EX_INDX) {
                    status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
                    status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_INDEXERROR;
                } else if (retval == PM_RET_EX_KEY) {
                    status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
                    status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_KEYERROR;
                } else if (retval == PM_RET_EX_MEM) {
                    status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
                    status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_MEMORYERROR;
                } else if (retval == PM_RET_EX_NAME) {
                    status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
                    status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_NAMEERROR;
                } else if (retval == PM_RET_EX_SYNTAX) {
                    status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
                    status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_SYNTAXERROR;
                } else if (retval == PM_RET_EX_SYS) {
                    status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
                    status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_SYSTEMERROR;
                } else if (retval == PM_RET_EX_TYPE) {
                    status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
                    status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_TYPEERROR;
                } else if (retval == PM_RET_EX_VAL) {
                    status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
                    status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_VALUEERROR;
                } else if (retval == PM_RET_EX_STOP) {
                    status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
                    status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_STOPITERATION;
                } else if (retval == PM_RET_EX_WARN) {
                    status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
                    status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_WARNING;
                } else {
                    status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
                    status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_UNKNOWNERROR;
                }
                // Get file ID and line number of error (if one)
                status.ErrorFileID  = gVmGlobal.errFileId;
                status.ErrorLineNum = gVmGlobal.errLineNum;
            } else {
                status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
                status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_VMINITERROR;
            }

            // Update status object
            FlightPlanStatusSet(&status);
        }
    }
}

/**
 * Function called in response to object updates.
 * Used to force kill the VM thread.
 */
static void objectUpdatedCb(UAVObjEvent *ev)
{
    FlightPlanControlData controlData;
    FlightPlanStatusData statusData;

    // If the object updated was the FlightPlanControl execute requested action
    if (ev->obj == FlightPlanControlHandle()) {
        // Get data
        FlightPlanControlGet(&controlData);
        // Execute command
        if (controlData.Command == FLIGHTPLANCONTROL_COMMAND_START) {
            // Start VM task if not running already
            if (taskHandle == NULL) {
                xTaskCreate(flightPlanTask, (signed char *)"FlightPlan", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
                PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_FLIGHTPLAN, taskHandle);
            }
        } else if (controlData.Command == FLIGHTPLANCONTROL_COMMAND_KILL) {
            // Force kill VM task if it is already running
            // (NOTE: the STOP command is preferred as it allows the script to terminate without killing the VM)
            if (taskHandle != NULL) {
                // Kill VM
                PIOS_TASK_MONITOR_UnregisterTask(TASKINFO_RUNNING_FLIGHTPLAN);
                vTaskDelete(taskHandle);
                taskHandle = NULL;
                // Update status object
                statusData.Status       = FLIGHTPLANSTATUS_STATUS_STOPPED;
                statusData.ErrorFileID  = 0;
                statusData.ErrorLineNum = 0;
                statusData.ErrorType    = FLIGHTPLANSTATUS_ERRORTYPE_NONE;
                statusData.Debug[0]     = 0.0;
                statusData.Debug[1]     = 0.0;
                FlightPlanStatusSet(&statusData);
            }
        }
    }
}

/**
 * @}
 * @}
 */
